#include "point2pixel_pin.h"
Point2PixelPin::Point2PixelPin(): Point2Pixel<pcl::PointXYZI>()
{
}

Point2PixelPin::~Point2PixelPin(){
}

cv::Point2d Point2PixelPin::loop(const pcl::PointXYZI& point) {
    //外参
    pcl::PointXYZI pt;
    pt = pcl::transformPoint(point, transform_A);

    // 按照线性模型，归一化(f=1)图像坐标系
    double tmpxC = pt.x / pt.z;
    double tmpyC = pt.y / pt.z;
    cv::Point2d planepointsC;

    double r2 = tmpxC * tmpxC + tmpyC * tmpyC;//归一化平面的极坐标轴长
    double tmpdist = 1 + distCoeffs.at<double>(0) * r2 + distCoeffs.at<double>(1) * r2 * r2 +
                     distCoeffs.at<double>(4) * r2 * r2 * r2;
    planepointsC.x = tmpxC * tmpdist + 2 * distCoeffs.at<double>(2) * tmpxC * tmpyC +
                     distCoeffs.at<double>(3) * (r2 + 2 * tmpxC * tmpxC);
    planepointsC.y = tmpyC * tmpdist + distCoeffs.at<double>(2) * (r2 + 2 * tmpyC * tmpyC) +
                     2 * distCoeffs.at<double>(3) * tmpxC * tmpyC;
    planepointsC.x = cameraMatrix.at<double>(0, 0) * planepointsC.x + cameraMatrix.at<double>(0, 2);
    planepointsC.y = cameraMatrix.at<double>(1, 1) * planepointsC.y + cameraMatrix.at<double>(1, 2);

    return planepointsC;
}


